#include "tim.h"
#include "main.h"
#include "run.h"
extern int key, data3, data4;
int i = 0;

void moca1go(void) // 左前轮前进
{
	// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
	// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);
}

void moca2go(void)
{
	// HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);
	// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_RESET);
}

void left1go(void) // 左前轮前进
{
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
}

void right1go(void) // 右前轮前进
{
	// HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_SET);
	// HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_RESET);

	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_SET);
}

void right1back(void)
{
	// HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_SET);
	// HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_RESET);
}

void left2go(void) // 左后轮前进
{
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET);
}

void left2back(void)
{
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET);
}

void right2go(void) // 右后轮前进
{
	// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6,GPIO_PIN_SET);
	// HAL_GPIO_WritePin(GPIOA,GPIO_PIN_7,GPIO_PIN_RESET);

	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_SET);
}

void left1back(void)
{
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, GPIO_PIN_RESET);
}

void left1stop(void)
{
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
}

void right1stop(void)
{
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_RESET);
}

void left2stop(void)
{
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET);
}

void right2back(void)
{
	// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_SET);
	// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_RESET);

	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_SET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_RESET);
}

void right2stop(void)
{
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_RESET);
}

void stop(void) // 电机不转
{
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET);
}

void go(int v, int b, int n, int m) // 前进
{
	left2go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, v);
	right2go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, b);
	left1go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, n);
	right1go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, m);
}

void back(int v, int b, int n, int m) // 后退
{
	left2back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, v);
	right2back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, b);
	left1back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, n);
	right1back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, m);
}

void right(int v, int b, int n, int m) // 后退
{
	left2go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, v);
	right2back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, b);
	left1back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, n);
	right1go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, m);
}

void left(int v, int b, int n, int m) // 后退
{
	left2back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, v);
	right2go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, b);
	left1go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, n);
	right1back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, m);
}

void rightspin(int v, int b, int n, int m) //
{
	left2go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, v);
	right2back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, b);
	left1go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, n);
	right1back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, m);
}

void leftspin(int v, int b, int n, int m) //
{
	left2back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1, v);
	right2go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, b);
	left1back();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3, n);
	right1go();
	__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4, m);
}

void shoot(int a, int b)
{
	moca1go();
	__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, a);
	moca2go();
	__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_2, b);
}
